
// import haxe.ds.Array;
// import oimo.common.MathUtil;
// import oimo.common.Setting;
// import oimo.common.Transform;
// import oimo.common.Vec3;
// import oimo.m.Vec3;
// import oimo.m.M;

import { Vec3 } from "../../../common/Vec3";
import { M } from "../../../common/M";
import { Setting } from "../../../common/Setting";
import { Transform } from "../../../common/Transform";
import { ManifoldPoint } from "./ManifoldPoint";
import { MathUtil } from "../../../common/MathUtil";

/**
 * A contact manifold holds collision data of a pair of shapes.
 */

export class Manifold {
	_normal: Vec3;
	_tangent: Vec3;
	_binormal: Vec3;
	_numPoints: number;
	_points: Array<ManifoldPoint>;

	constructor() {
		this._normal = new Vec3();
		this._tangent = new Vec3();
		this._binormal = new Vec3();
		this._numPoints = 0;
		this._points = new Array<ManifoldPoint>(Setting.maxManifoldPoints);
		for (let i = 0; i < Setting.maxManifoldPoints; ++i) {
			this._points[i] = new ManifoldPoint();
		}
	}

	// --- internal ---

	public _clear(): void {
		for (let i = 0; i < this._numPoints; ++i) {
			this._points[i]._clear();
		}
		this._numPoints = 0;
	}

	public _buildBasis(normal: Vec3): void {
		this._normal.copyFrom(normal);
		let nx: number = normal.x;
		let ny: number = normal.y;
		let nz: number = normal.z;
		let nx2: number = nx * nx;
		let ny2: number = ny * ny;
		let nz2: number = nz * nz;
		let tx: number;
		let ty: number;
		let tz: number;
		let bx: number;
		let by: number;
		let bz: number;
		switch (M.compare3min(nx2, ny2, nz2)) {
			case -1:
				{
					let invL: number = 1 / MathUtil.sqrt(ny2 + nz2);
					tx = 0;
					ty = -nz * invL;
					tz = ny * invL;
					bx = ny * tz - nz * ty;
					by = -nx * tz;
					bz = nx * ty;
				}
				break;
			case 0:
				{
					let invL: number = 1 / MathUtil.sqrt(nx2 + nz2);
					tx = nz * invL;
					ty = 0;
					tz = -nx * invL;
					bx = ny * tz;
					by = nz * tx - nx * tz;
					bz = -ny * tx;
				}
				break;
			case 1:
				{
					let invL: number = 1 / MathUtil.sqrt(nx2 + ny2);
					tx = -ny * invL;
					ty = nx * invL;
					tz = 0;
					bx = -nz * ty;
					by = nz * tx;
					bz = nx * ty - ny * tx;
				}
				break;
		}
		this._tangent.init(tx, ty, tz);
		this._binormal.init(bx, by, bz);
	}

	public _updateDepthsAndPositions(tf1: Transform, tf2: Transform): void {
		var diff: Vec3 = new Vec3();
		for (let i = 0; i < this._numPoints; ++i) {
			var p: ManifoldPoint = this._points[i];
			p._localPos1.mulMat3to(tf1._rotation, p._relPos1);
			p._localPos2.mulMat3to(tf2._rotation, p._relPos2);
			p._relPos1.addTo(tf1._position, p._pos1);
			p._relPos2.addTo(tf2._position, p._pos2);

			M.vec3_sub(diff, p._pos1, p._pos2);
			var dotN: number = M.vec3_dot(diff, this._normal);
			p._depth = -dotN;
		}
	}

	// --- public ---

	/**
	 * Returns the normal Array of the contact manifold. The normal Array has unit
	 * length and is perpendicular to the contact plane.
	 */
	public getNormal(): Vec3 {
		return this._normal.clone();
	}

	/**
	 * Sets `normal` to the normal Array of the contact manifold. The normal Array has
	 * unit length and is perpendicular to the contact plane.
	 *
	 * This does not create a new instance of `Vec3`.
	 */
	public getNormalTo(normal: Vec3): void {
		normal.copyFrom(this._normal);
	}

	/**
	 * Returns the tangent Array of the contact manifold. The tangent Array has unit
	 * length and is perpendicular to the normal Array.
	 */
	public getTangent(): Vec3 {
		return this._tangent.clone();
	}

	/**
	 * Sets `tangent` to the tangent Array of the contact manifold. The tangent Array has
	 * unit length and is perpendicular to the normal Array.
	 *
	 * This does not create a new instance of `Vec3`.
	 */
	public getTangentTo(tangent: Vec3): void {
		tangent.copyFrom(this._tangent);
	}

	/**
	 * Returns the binormal Array of the contact manifold. The binormal Array has unit
	 * length and is perpendicular to both the normal and the tangent Array.
	 */
	public getBinormal(): Vec3 {
		return this._binormal.clone();
	}

	/**
	 * Sets `binormal` to the binormal Array of the contact manifold. The binormal Array has
	 * unit length and is perpendicular to both the normal and the tangent Array.
	 *
	 * This does not create a new instance of `Vec3`.
	 */
	public getBinormalTo(binormal: Vec3): void {
		binormal.copyFrom(this._binormal);
	}

	/**
	 * Returns the manifold point Array of the contact manifold. Note that **only the first
	 * `Manifold.getNumPoints` elements of the Array are in use**, and the manifold points may
	 * be disabled (see `ManifoldPoint.isEnabled`).
	 */
	public getPoints(): Array<ManifoldPoint> {
		return this._points;
	}

	/**
	 * Returns the number of existing manifold points.
	 */
	public getNumPoints(): number {
		return this._numPoints;
	}

}
